#include "omni.h"
#include <stdint.h>
#include <string.h>
#include <arm_math.h>

#include "dji.h"
#include "omni.h"
#include "rtklibc.h"
#include "user_lib.h"
#ifndef RADIAN_COEF
    #define RADIAN_COEF 57.3f
#endif

#define OMNI_VAL_LIMIT(val, min, max) \
  do                                 \
  {                                  \
    if ((val) <= (min))              \
    {                                \
      (val) = (min);                 \
    }                                \
    else if ((val) >= (max))         \
    {                                \
      (val) = (max);                 \
    }                                \
  } while (0)
/**
  * @brief mecanum glb_chassis velocity decomposition.F:forword; B:backword; L:left; R:right
  * @param input : ccx=+vx(mm/s)  ccy=+vy(mm/s)  ccw=+vw(deg/s)
  *        output: every wheel speed(rpm)
  * @note  1=FR 2=FL 3=BL 4=BR
  */
void omni_calculate(struct omni *omn)
{
    // static float rotate_ratio_fr;
    // static float rotate_ratio_fl;
    // static float rotate_ratio_bl;
    // static float rotate_ratio_br;
    static float wheel_rpm_ratio;
    static float wheel_speed_ops[4];

    // rotate_ratio_fr = ((omn->param.wheelbase + omn->param.wheeltrack) / 2.0f - omn->param.rotate_x_offset + omn->param.rotate_y_offset) / RADIAN_COEF;
    // rotate_ratio_fl = ((omn->param.wheelbase + omn->param.wheeltrack) / 2.0f - omn->param.rotate_x_offset - omn->param.rotate_y_offset) / RADIAN_COEF;
    // rotate_ratio_bl = ((omn->param.wheelbase + omn->param.wheeltrack) / 2.0f + omn->param.rotate_x_offset - omn->param.rotate_y_offset) / RADIAN_COEF;
    // rotate_ratio_br = ((omn->param.wheelbase + omn->param.wheeltrack) / 2.0f + omn->param.rotate_x_offset + omn->param.rotate_y_offset) / RADIAN_COEF;

    wheel_rpm_ratio = 60.0f / (omn->param.wheel_perimeter*PI) * (MOTOR_DECELE_RATIO)*100;

    // OMNI_VAL_LIMIT(omn->speed.vx, -MAX_CHASSIS_VX_SPEED, MAX_CHASSIS_VX_SPEED); //mm/s
    // OMNI_VAL_LIMIT(omn->speed.vy, -MAX_CHASSIS_VY_SPEED, MAX_CHASSIS_VY_SPEED); //mm/s
    // OMNI_VAL_LIMIT(omn->speed.vw, -MAX_CHASSIS_VW_SPEED, MAX_CHASSIS_VW_SPEED); //deg/s

    float wheel_rpm[4];
    float max = 0;


    wheel_rpm[0] = (   omn->speed.vx - omn->speed.vy - omn->speed.vw * ((LENGTH_A+LENGTH_B)/RADIAN_COEF))*wheel_rpm_ratio;//left//x，y方向速度,w底盘转动速度
    wheel_rpm[1] = (   -omn->speed.vx - omn->speed.vy - omn->speed.vw * ((LENGTH_A+LENGTH_B)/RADIAN_COEF))*wheel_rpm_ratio;//forward
    wheel_rpm[2] = (  omn->speed.vx + omn->speed.vy - omn->speed.vw * ((LENGTH_A+LENGTH_B)/RADIAN_COEF))*wheel_rpm_ratio;//right
    wheel_rpm[3] = (  -omn->speed.vx + omn->speed.vy - omn->speed.vw * ((LENGTH_A+LENGTH_B)/RADIAN_COEF))*wheel_rpm_ratio;//back

    //find max item
    for (uint8_t i = 0; i < 4; i++)
    {
        if (fabs(wheel_rpm[i]) > max)
        {
            max = fabs(wheel_rpm[i]);
        }
    }

    //equal proportion
    if (max > MAX_WHEEL_RPM)
    {
        float rate = MAX_WHEEL_RPM / max;
        for (uint8_t i = 0; i < 4; i++)
        {
            wheel_rpm[i] *= rate;
            wheel_speed_ops[i] = RPM_2_ANGLE_PER_SEC * SPEED_SMOOTH_COEF * (float)wheel_rpm[i];
        }
    }
    rt_memcpy(omn->wheel_rpm, wheel_rpm, 4 * sizeof(float));
    rt_memcpy(omn->wheel_ops,wheel_speed_ops,4*sizeof(float));
}

